/*
 * mod_pid.h
 *
 *  Created on: 2025 Feb 18
 *      Author: SFLY
 */

#ifndef XTFOCLIB_MODULES_MOD_PID_H_
#define XTFOCLIB_MODULES_MOD_PID_H_

/******************************************************************************/
/* Global type definitions ('typedef')                                        */
/******************************************************************************/

/** incremental PID ***********************************************************/
typedef struct {
    float Kp;          /** proprotion gain */
    float Ki;          /** integration gain */
    float Output;      /** output */
    float OutMin;      /** minimum output */
    float OutMax;      /** maximum output */
    float Err1;        /** last error */
    float AbsErrMax;   /** maximum absolute error */
} stc_inc_pid_t;

///** position PID ***********************************************************/
//typedef struct {
//    float Kp;          /** proportion gain */
//    float Ki;          /** integration gain */
//    float Output;      /** output */
//    float OutMin;      /** minumum output */
//    float OutMax;      /** maximum output */
//    float Iout;        /** store integration value */
//    float Err1;        /** last error */
////    float fb_cur[100];
////    float Err_cur[100];
////    int isfirstflag;
////    int issecflag;
//    float AbsErrMax;   /** maximum absolute error */
//} stc_pos_pid_t;

// 定义PID结构体
typedef struct {
    float Kp;  // 比例系数
    float Ki;  // 积分系数
    float Kd;  // 微分系数
    float OutMin;  // 输出最小值
    float OutMax;  // 输出最大值
    float AbsErrMax;  // 最大误差值
    float ErrSeparation; // 积分分离阈值
    float Output;  // 输出值
    float Iout;  // 积分项输出
    float Err1;  // 上一次的误差值
    float Err2;  // 上上次的误差值
} stc_pos_pid_t;




typedef struct{
float pos_Kp;
float spd_Kd;
float Output;
}stc_mit_pid_t;//基于MIT协议的PID控制器

/**
 ******************************************************************************
 ** \brief initialize MIT PI regulator
 **
 ** \param [in] stc_pid_cfg_t stcCfg: configuration parameters
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     none
 ******************************************************************************/
void PID_InitMITPid(float Kp,float Kd, stc_mit_pid_t *pstc);

/**
 ******************************************************************************
 ** \brief incremental PI regulator
 **
 ** \param [in] float Ref:  reference
 **             float Fb:   feedback
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     float Output
 ******************************************************************************/
float PID_MITPid(float Pos_Ref, float Pos_Obser,float Spd_Ref,float Spd_Obser, float Torque_Ref,stc_mit_pid_t *pstc);
/**
 ******************************************************************************
 ** \brief initialize incremental PI regulator
 **
 ** \param [in] stc_pid_cfg_t stcCfg: configuration parameters
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     none
 ******************************************************************************/
void PID_InitIncPid(float Kp,float Ki,float OutMin,float OutMax,float ErrMax, stc_inc_pid_t *pstc) ;

/**
 ******************************************************************************
 ** \brief incremental PI regulator
 **
 ** \param [in] float Ref:  reference
 **             float Fb:   feedback
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     float Output
 ******************************************************************************/
float PID_IncPid(float Ref, float Fb, stc_inc_pid_t *pstc);

/**
 ******************************************************************************
 ** \brief initialize position PI regulator
 **
 ** \param [in] stc_pid_cfg_t stcCfg: configuration parameters
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     none
 ******************************************************************************/
//void PID_InitPosPid(float Kp,float Ki,float OutMin,float OutMax,float ErrMax, stc_pos_pid_t *pstc);
void PID_InitPosPid(float Kp, float Ki, float Kd, float OutMin, float OutMax, float ErrMax, float ErrSeparation, stc_pos_pid_t *pstc);

/**
 ******************************************************************************
 ** \brief position PI regulator
 **
 ** \param [in] float Ref:  reference
 **             float Fb:   feedback
 **             stc_inc_pid_t* pstc: pointer to PID regulator
 **
 ** \retval     float Output
 ******************************************************************************/
//float PID_PosPid(float Ref, float Fb, stc_pos_pid_t *pstc);
float PID_PosPid(float Ref, float Fb, stc_pos_pid_t *pstc);
#endif /* XTFOCLIB_MODULES_MOD_PID_H_ */
